#ifndef __ROBOT_MODEL_BASE_H__
#define __ROBOT_MODEL_BASE_H__

#include <ros/ros.h>
#include <ros/package.h>
#include <geometry_msgs/PolygonStamped.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <iostream>
using namespace std;

#include "common/file_manager/yaml_file.h"
#include "common/file_manager/file_manager.h"

struct RobotModel {
    string robot_name;
    string chassis_model;
    float robot_radius;        // 机器人半径
    vector<geometry_msgs::Point> robot_footprints; // 机器人足迹
    float inscribed_circle_radius;      // 机器人内切圆半径
    float circumscribed_circle_radius;  // 机器人外接圆半径
};


class ModelBase {
    public:
        ModelBase(string configFile);
        ~ModelBase(){};

        // 解析机器人模型
        void parserRobotModel();
        // 获取机器人模型
        RobotModel getRobotModel();
        // 获取多边形轮廓
        visualization_msgs::Marker PolygonMarker(const std::string& frame_id, vector<geometry_msgs::Point> polygon, string ns);
        geometry_msgs::PolygonStamped toPolygonStamped(const std::string& frame_id);
        // 获取圆形轮廓
        visualization_msgs::Marker CircleMarker(const std::string& frame_id, float radius, string ns);
        // 获取机器人模型
        visualization_msgs::MarkerArray getRobotModelMarkerArray(const std::string& frame_id);

        // 计算点到原点的距离
        float distanceToOrigin(const geometry_msgs::Point& point);
        // 计算点到线段的垂直距离（点到线段的最短距离）
        float pointToLineDistance(const geometry_msgs::Point& p, const geometry_msgs::Point& p1, const geometry_msgs::Point& p2);
        // 计算最大内切圆半径和最小内切圆半径
        void calculateIncircleRadius(const std::vector<geometry_msgs::Point>& robot_footprints, float& max_radius, float& min_radius);

    private:
        string configFile_;
        RobotModel robotModel_;
};

#endif